"""
RobotCar with H-bridge motor controller 
"""

from time import sleep
from gpiozero import  Robot,Motor
from gpiozero.pins.pigpio import PiGPIOFactory



class RobotCar:

    FORWARD = 0
    BACKWARD = 1
    STOP = 2
    LEFT_FORWARD = 3   # Moves forwards in an arc by setting different speeds.  left_speed<1, right_speed = 1
    RIGHT_FORWARD  = 4 # Moves forwards in an arc by setting different speeds.  left_speed=1, right_speed <1
    LEFT_FORWARD_STEP = 5
    RIGHT_FORWARD_STEP = 6
    SPIN_LEFT=7  # Make the robot turn left by running the right motor forward and left motor backward
    SPIN_RIGHT=8 # Make the robot turn right by running the left motor forward and right motor backward.
    GO=9

    ADD_SPEED=101
    SUB_SPEED=102
    ADD_LEFT_SPEED=201
    SUB_LEFT_SPEED=202
    ADD_RIGHT_SPEED=301
    SUB_RIGHT_SPEED=302
    
    SPEED_100=1
    SPEED_90=0.9
    SPEED_80=0.8
    SPEED_75=0.75
    SPEED_70=0.7
    SPEED_60=0.6
    SPEED_50=0.5
    SPEED_40=0.4
    SPEED_30=0.3
    SPEED_25=0.25
    SPEED_10=0.1
 
    def __init__(self, left_IN_P,left_IN_N,right_IN_P,right_IN_N):
        self.MOTOR_STATE = RobotCar.STOP
        self.left_speed= RobotCar.SPEED_50 # the default speed 
        self.right_speed= RobotCar.SPEED_50 # the default speed
        self.step_speed=0.05
        self.speed_makeup=0.0 # 补偿设备差，使左右车轮速度一致
        self.min_speed=0.3
        # the speed hard PWM with pigpio
        self.robot=Robot(left=(left_IN_P,left_IN_N),right=(right_IN_P,right_IN_N),pin_factory=PiGPIOFactory())
        self.robot.value=(0,0)
        self.DICT_CMD_ACTION = {RobotCar.FORWARD: self.forward,
                                RobotCar.BACKWARD: self.backward,
                                RobotCar.STOP: self.robot.stop,
                                RobotCar.LEFT_FORWARD: self.forward_left,
                                RobotCar.RIGHT_FORWARD: self.forward_right,
                                RobotCar.LEFT_FORWARD_STEP: self.forward_left_step,
                                RobotCar.RIGHT_FORWARD_STEP: self.forward_right_step,
                                RobotCar.SPIN_LEFT: self.robot.left,
                                RobotCar.SPIN_RIGHT: self.robot.right,
                                # speed
                                RobotCar.ADD_SPEED: self.add_speed,
                                RobotCar.SUB_SPEED: self.sub_speed,
                                RobotCar.ADD_LEFT_SPEED: self.add_left_speed,
                                RobotCar.SUB_LEFT_SPEED: self.sub_left_speed,
                                RobotCar.ADD_RIGHT_SPEED: self.add_right_speed,
                                RobotCar.SUB_RIGHT_SPEED: self.sub_right_speed,
                                }
   
    def forward(self):
        if self.left_speed<0:
           self.left_speed =-self.left_speed
        if self.right_speed<0:
           self.right_speed=-self.right_speed
        if self.left_speed> self.right_speed:
           self.right_speed=  self.left_speed + self.speed_makeup 
        else:
           self.left_speed=  self.right_speed + self.speed_makeup
        # min speed
        if self.left_speed<self.min_speed:
           self.left_speed=self.min_speed
        if self.right_speed<self.min_speed:
           self.right_speed=self.min_speed
        self.robot.value=(self.left_speed,self.right_speed)
        #self.robot.forward()
   
    def backward(self):
        if self.left_speed>0:
           self.left_speed =-self.left_speed
        if self.right_speed>0:
           self.right_speed=-self.right_speed
        if self.left_speed< self.right_speed:
           self.right_speed =  self.left_speed -self.speed_makeup
        else:
           self.left_speed=  self.right_speed  -self.speed_makeup
        # min speed
        if self.left_speed>-self.min_speed:
           self.left_speed =-self.min_speed
        if self.right_speed>-self.min_speed:
           self.right_speed=-self.min_speed   
        self.robot.value=(self.left_speed,self.right_speed)
        #self.robot.backward()

    def forward_right(self):
        """ 
         Moves forwards in an arc by setting different speeds
          left_speed=1, right_speed<1
        """
        if self.MOTOR_STATE==RobotCar.STOP:
           self.left_speed = RobotCar.SPEED_50 # the default speed 
        if (self.left_speed<0):
           self.left_speed=-self.left_speed
        if self.left_speed<self.min_speed:
           self.left_speed=self.min_speed
        self.robot.value=(self.left_speed,0)
        #self.robot.forward(curve_left=1, curve_right=0)

    def forward_left(self):
        """
         Moves forwards in an arc by setting different speeds. 
            left_speed<1, right_speed = 1
        """
        if self.MOTOR_STATE==RobotCar.STOP:
           self.right_speed= RobotCar.SPEED_50 # the default speed 
        if (self.right_speed<0):
           self.right_speed=-self.right_speed
        if self.right_speed<self.min_speed:
           self.right_speed=self.min_speed
        self.robot.value=(0,self.right_speed)
        #self.robot.forward(curve_left=0, curve_right=1)
      
    def forward_left_step(self):
        self.forward_left()
        sleep(0.1)
        self.robot.stop()
        self.MOTOR_STATE= RobotCar.STOP

    def forward_right_step(self):
        self.forward_right()
        sleep(0.1)
        self.robot.stop()
        self.MOTOR_STATE= RobotCar.STOP

    def go(self,left_speed,right_speed):
        """
        controls motors in both directions independently using different positive/negative speeds. 
            -1<= leftSpeed,rightSpeed <= 1
        """
        speed_changed=False
        if (self.left_speed!=left_speed):
            self.left_speed=left_speed
            speed_changed=True
        if (self.right_speed!=right_speed):
            self.right_speed=right_speed
            speed_changed=True
        if (speed_changed):
            self.robot.value=(self.left_speed,self.right_speed)
     
    def add_speed(self):
        (self.left_speed,self.right_speed)= self.robot.value
        self.left_speed +=self.step_speed
        self.right_speed +=self.step_speed
        if self.left_speed>1:
           self.left_speed=1
        if self.right_speed>1:
           self.right_speed=1
        self.robot.value=(self.left_speed,self.right_speed)
  
    def sub_speed(self):
        (self.left_speed,self.right_speed)= self.robot.value
        self.left_speed -=self.step_speed
        self.right_speed -=self.step_speed
        if self.right_speed<-1:
           self.right_speed=-1
        if self.left_speed<-1:
           self.left_speed=-1
        self.robot.value=(self.left_speed,self.right_speed)

    def add_left_speed(self):
        self.left_speed=self.robot.left_motor.value
        self.left_speed +=self.step_speed
        if self.left_speed>1:
           self.left_speed=1
        self.robot.left_motor.value=self.left_speed
      
    def sub_left_speed(self):
        self.left_speed=self.robot.left_motor.value
        self.left_speed -=self.step_speed
        if self.left_speed<-1:
           self.left_speed=-1
        self.robot.left_motor.value=self.left_speed
    
    def add_right_speed(self):
        self.right_speed=self.robot.right_motor.value
        self.right_speed +=self.step_speed
        if self.right_speed>1:
           self.right_speed=1
        self.robot.right_motor.value=self.right_speed
     
    def sub_right_speed(self):
        self.right_speed=self.robot.right_motor.value
        self.right_speed -=self.step_speed
        if self.right_speed<-1:
           self.right_speed=-1
        self.robot.right_motor.value=self.right_speed
 
    def action(self, cmd_to_motor):
        #if cmd_to_motor in [RobotCar.FORWARD,RobotCar.BACKWARD,
        #                    RobotCar.LEFT_FORWARD,RobotCar.RIGHT_FORWARD,
        #                    RobotCar.SPIN_LEFT,RobotCar.SPIN_RIGHT]:
        #    # 不需要连续命令的状态
        #    if (cmd_to_motor != self.MOTOR_STATE):
        #       self.MOTOR_STATE = cmd_to_motor
        #       self.DICT_CMD_ACTION[cmd_to_motor]()
        #else:
        #   # 需要连续命令的状态
        # 不判断，减少复杂度
        self.MOTOR_STATE = cmd_to_motor
        self.DICT_CMD_ACTION[cmd_to_motor]()
           
     
if  __name__=="__main__":
    car=RobotCar(6,5,17,27) 
    car.action(RobotCar.FORWARD)
    sleep(1)
    car.action(RobotCar.STOP)
    sleep(1)
    car.action(RobotCar.BACKWARD)
    sleep(1)
   # car.action(RobotCar.STOP)
  #  sleep(1)
    #car.action(RobotCar.LEFT)
    #sleep(1)
    #car.action(RobotCar.RIGHT)
    #sleep(1)
    car.action(RobotCar.RIGHT_FORWARD_STEP)
    sleep(1)
    #car.action(RobotCar.STOP)
    #sleep(1)
   
     
